#ifndef H_MAG
#define H_MAG

#include "Arduino.h"

class Mag
{
private:
  static const byte CRA=0,CRB=1,MODE=2,XYZ=3;
  static const byte // relative to CRA
  NORMAL=0,
  BIASPOS=1, // x&y=1.16 Gauss z=1.08 Gauss
  BIASNEG=2; 
  static const byte // relative to CRA
  RATE0P75 = (0<<2),
  RATE1P5  = (1<<2),
  RATE3    = (2<<2),
  RATE7P5  = (3<<2),
  RATE15   = (4<<2),
  RATE30   = (5<<2),
  RATE75   = (6<<2);
  static const byte // relative to CRA
  SAMPLE1  = (0<<5),
  SAMPLE2  = (1<<5),
  SAMPLE4  = (2<<5),
  SAMPLE8  = (3<<5);

  static const byte // relative to CRB value relative to 1 Gauss (earth normal is 0.2 to 0.7, eg. italy 0.4Gauss)
  SCALE1370 = (0<<5),
  SCALE1090 = (1<<5),
  SCALE820  = (2<<5),
  SCALE660  = (3<<5),
  SCALE440  = (4<<5),
  SCALE390  = (5<<5),
  SCALE330  = (6<<5),
  SCALE230  = (7<<5);

  static const byte // relative to MODE
  CONT = 0,
  SINGLE = 1,
  IDLE = 2;

  int xi,yi,zi;
  int count;
  byte state;

public:
  int errorCount;
  byte errorCode;
  float vmin[3],vmax[3],scale[3];
  float x,y,z; 

  byte setup()
  {
    byte ret;
#ifdef MAG_PRESENT
#if defined(HMC5843) || defined(HMC5883L)
    delay(100);
    ret=fw.write(MAG_ADDR,CRA,SAMPLE1 | BIASPOS);  
    if (ret!=0) return -1;
    delay(50);
    ret=fw.write(MAG_ADDR,CRB,SCALE1090); // you can't use the default 1370 scale because BIAS is too strong 
    if (ret!=0) return -2;
    ret=fw.write(MAG_ADDR,MODE,SINGLE);
    if (ret!=0) return -3;

    delay(100);
    getRaw();
    scale[0]=abs(xi)/1.16*1090/1370; // different scale 1090 1370
    scale[1]=abs(yi)/1.16*1090/1370;
    scale[2]=abs(zi)/1.08*1090/1370; // BIAS is different for z (it is lower)

    ret=fw.write(MAG_ADDR,CRA,SAMPLE8 | RATE75);
    if (ret!=0) return -4;
    ret=fw.write(MAG_ADDR,CRB,SCALE1370);
    if (ret!=0) return -5;
    ret=fw.write(MAG_ADDR,MODE,CONT);
    if (ret!=0) return -6;


#endif
#endif
    for(byte i=0;i<3;i++)
      vmin[i]=vmax[i]=0;

    return 0;
  }

  void setState(byte s)
  {
    state=s;
    if (s==1) 
    {
      count=0;
      for(byte i=0;i<3;i++)
      {
        vmin[i]=1000;
        vmax[i]=-1000;
      }
    }
  }

  void save(void)
  {
    for(byte i=0;i<3;i++)
    {
      para.writeFloat(vmin[i]);
      para.writeFloat(vmax[i]);
      para.writeFloat(scale[i]);
    }
  }

  void load(void)
  {
    for(byte i=0;i<3;i++)
    {
      vmin[i]=para.readFloat();
      vmax[i]=para.readFloat();
      scale[i]=para.readFloat();
    }
  }

  void getRaw()
  {
    byte buff[3*sizeof(int)];


#if defined(HMC5883L)
    fw.readBuf(MAG_ADDR,XYZ,buff,3*sizeof(int));

    MAG_ORIENTATION(
    -((buff[0] << 8) | buff[1]),
    -((buff[4] << 8) | buff[5]),
    ((buff[2] << 8) | buff[3]) ); 

#elif defined(HMC5843)
    ret=fw.readBuf(MAG_ADDR,XYZ,buff,3*sizeof(int));

    if (ret==0)
    {
      MAG_ORIENTATION(
      ((buff[0] << 8) | buff[1]),
      ((buff[2] << 8) | buff[3]),
      ((buff[4] << 8) | buff[5]) ); 
    }
    else
    {
      errorCount++;
      errorCode=ret;
    }
#endif
  }

  byte update()
  {
    byte ret;
    getRaw();

    x=xi/scale[0];
    y=yi/scale[1];
    z=zi/scale[2];
    // after scale value are in Gauss

    if (state==1) // 1=search zero
    {
      // Search maximum
      if (x<vmin[0]) vmin[0]=x;
      if (y<vmin[1]) vmin[1]=y;
      if (z<vmin[2]) vmin[2]=z;
      if (x>vmax[0]) vmax[0]=x;
      if (y>vmax[1]) vmax[1]=y;
      if (z>vmax[2]) vmax[2]=z;

      count++;
      if (count>=30*50) // 30 seconds 50hz TODO check 50hz
      {
        state=0;
        saveAll();
        led.setOutput(BLINKNORMAL);
      }
    }

    // Value [-0.5;0.5]
    x=(x-vmin[0])/(vmax[0]-vmin[0])-0.5;
    y=(y-vmin[1])/(vmax[1]-vmin[1])-0.5;
    z=(z-vmin[2])/(vmax[2]-vmin[2])-0.5;
    return ret;
  }

} 
mag;

#endif


















